/*
 * FlightController_Configuration.cpp
 *
 *  Created on: Feb 3, 2014
 *      Author: ppks
 */

#include "common.h"
#include "FlightController_Configuration.h"
#include "FlightController_Defaults.h"
#include "FlightController.h"
#include "GPS_common.h"
#include "EEPROM.h"

/*****************************************************************************/
/* Constants Definition                                                      */
/*****************************************************************************/


/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/
GlobalInfo_t   ConfigInfo;
GlobalConfig_t FC_Config;

int16_t lookupPitchRollRC[5];       /* lookup table for expo & RC rate PITCH+ROLL */
int16_t lookupThrottleRC[11];       /* lookup table for expo & mid THROTTLE */

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: FC_LoadConfigDefaults
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FC_LoadConfigDefaults(void)
{
    uint8_t i;
#ifdef SUPPRESS_DEFAULTS_FROM_GUI
    /* do nothing */
#elif defined(MY_PRIVATE_DEFAULTS)
    /* #include MY_PRIVATE_DEFAULTS */
    /* do that at the last possible moment, so we can override virtually all defaults and constants */
#else
    /* Normal Defaults Update */
    /* TODO: create #defines for defaults */
    #if PID_CONTROLLER == 1
        FC_Config.PID[ROLL].P8     = 33;    FC_Config.PID[ROLL].I8     = 30;    FC_Config.PID[ROLL].D8     = 23;
        FC_Config.PID[PITCH].P8    = 33;    FC_Config.PID[PITCH].I8    = 30;    FC_Config.PID[PITCH].D8    = 23;
        FC_Config.PID[PIDLEVEL].P8 = 90;    FC_Config.PID[PIDLEVEL].I8 = 10;    FC_Config.PID[PIDLEVEL].D8 = 100;
    #elif PID_CONTROLLER == 2
        FC_Config.PID[ROLL].P8     = 28;    FC_Config.PID[ROLL].I8    = 10;     FC_Config.PID[ROLL].D8     = 7;
        FC_Config.PID[PITCH].P8    = 28;    FC_Config.PID[PITCH].I8    = 10;    FC_Config.PID[PITCH].D8    = 7;
        FC_Config.PID[PIDLEVEL].P8 = 30;    FC_Config.PID[PIDLEVEL].I8 = 32;    FC_Config.PID[PIDLEVEL].D8 = 0;
    #endif

    FC_Config.PID[YAW].P8      = 68;        FC_Config.PID[YAW].I8     = 45;     FC_Config.PID[YAW].D8     = 0;
    FC_Config.PID[PIDALT].P8   = 64;        FC_Config.PID[PIDALT].I8   = 25;    FC_Config.PID[PIDALT].D8   = 24;

    FC_Config.PID[PIDPOS].P8  = POSHOLD_P * 100;     FC_Config.PID[PIDPOS].I8    = POSHOLD_I * 100;       FC_Config.PID[PIDPOS].D8    = 0;
    FC_Config.PID[PIDPOSR].P8 = POSHOLD_RATE_P * 10; FC_Config.PID[PIDPOSR].I8   = POSHOLD_RATE_I * 100;  FC_Config.PID[PIDPOSR].D8   = POSHOLD_RATE_D * 1000;
    FC_Config.PID[PIDNAVR].P8 = NAV_P * 10;          FC_Config.PID[PIDNAVR].I8   = NAV_I * 100;           FC_Config.PID[PIDNAVR].D8   = NAV_D * 1000;

    FC_Config.PID[PIDMAG].P8   = 40;

    FC_Config.PID[PIDVEL].P8 = 0;      FC_Config.PID[PIDVEL].I8 = 0;    FC_Config.PID[PIDVEL].D8 = 0;

    /* Load RC Config Default Values */
    FC_Config.RC = (RCConfig_t)RC_TUNING_CONFIG_DEFAULTS;

    /* Initialize BOX configuration to 0 */
    for(i  = 0; i < CHECKBOXITEMS; i++)
    {
        FC_Config.BOX.activate[i] = 0;
    }

    /* Initialize MISC Parameters */
    FC_Config.MISC.powerTrigger1 = 0;
#endif /* SUPPRESS_DEFAULTS_FROM_GUI */

#if defined(SERVO)
    static int8_t sr[8] = SERVO_RATES;
#ifdef SERVO_MIN
    static int16_t smin[8] = SERVO_MIN;
    static int16_t smax[8] = SERVO_MAX;
    static int16_t smid[8] = SERVO_MID;
#endif

    for(i = 0; i < 8; i++)
    {
#ifdef SERVO_MIN
        FC_Config.MISC.servoConf[i].min = smin[i];
        FC_Config.MISC.servoConf[i].max = smax[i];
        FC_Config.MISC.servoConf[i].middle = smid[i];
#else /* SERVO_MIN */
        FC_Config.MISC.servoConf[i].min = 1020;
        FC_Config.MISC.servoConf[i].max = 2000;
        FC_Config.MISC.servoConf[i].middle = 1500;
#endif /* SERVO_MIN */
        FC_Config.MISC.servoConf[i].rate = sr[i];
    }
#endif /* defined(SERVO) */

#ifdef FIXEDWING
    FC_Config.RC.dynThrPID = 50;
    FC_Config.RC.rcExpo8   =  0;
#endif

#if defined(GYRO_SMOOTHING)
    {
        uint8_t s[3] = GYRO_SMOOTHING;
        for (uint8_t i = 0; i < 3; i++)
        {
            FC_Config.MISC.Smoothing[i] = s[i];
        }
    }
#endif
#if defined (FAILSAFE)
    FC_Config.MISC.failsafe_throttle = FAILSAFE_THROTTLE;
#endif
#ifdef VBAT_ENABLED
    FC_Config.MISC.vbatscale = VBATSCALE;
    FC_Config.MISC.vbatlevel_warn1 = VBATLEVEL_WARN1;
    FC_Config.MISC.vbatlevel_warn2 = VBATLEVEL_WARN2;
    FC_Config.MISC.vbatlevel_crit = VBATLEVEL_CRIT;
#endif
#ifdef POWERMETER
    FC_Config.MISC.pint2ma = PINT2mA;
#endif
#ifdef POWERMETER_HARD
    FC_Config.MISC.psensornull = PSENSORNULL;
#endif
#ifdef MMGYRO
    FC_Config.MISC.mmgyro = MMGYRO;
#endif
#if defined(ARMEDTIMEWARNING)
    FC_Config.MISC.armedtimewarning = ARMEDTIMEWARNING;
#endif
    FC_Config.MISC.minthrottle = MINTHROTTLE;
#if MAG_INSTALLED
    FC_Config.MISC.mag_declination = (int16_t)(MAG_DECLINATION * 10);
#endif
#ifdef GOVERNOR_P
    FC_Config.MISC.governorP = GOVERNOR_P;
    FC_Config.MISC.governorD = GOVERNOR_D;
#endif
#if defined(MY_PRIVATE_DEFAULTS)
    #include MY_PRIVATE_DEFAULTS
#endif
}

/*******************************************************************************
* Function Name: FC_VerifyConfigIntegrity
********************************************************************************
* Summary:
*  This function verifies the checksums for each configuration set and return
*  false if any of the configurations has checksum mismatch
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
bool FC_VerifyConfigIntegrity(void)
{
    uint8_t currentSet;

    for (currentSet = 0; currentSet < FC_CONFIG_MAX_SET_NUMBER; currentSet++)
    {
        if (EEPROM_ReadParams(currentSet) == false)
        {
            /* Terminate the check here and return false */
            return false;
        }
    }

    /* If we got here, the integrity check passed, so return true */
    return true;
}

/*******************************************************************************
* Function Name: FC_CalculateRCLookupVectors
********************************************************************************
* Summary:
*  Calculates LUTs for RC commands and store them in global arrays.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FC_CalculateRCLookupVectors(void)
{
    uint8_t i;

    /* 500/128 = 3.90625    3.9062 * 3.9062 = 15.259   1526*100/128 = 1192 */
    for(i = 0; i < 5; i++)
    {
        lookupPitchRollRC[i] = (1526 + FC_Config.RC.rcExpo8 * (i * i - 15)) * i * (int32_t)FC_Config.RC.rcRate8 / 1192;
    }

    for(i = 0; i < 11; i++)
    {
        int16_t tmp = 10 * i - FC_Config.RC.thrMid8;
        uint8_t y = 1;
        if (tmp > 0)
        {
            y = 100-FC_Config.RC.thrMid8;
        }
        if (tmp < 0)
        {
            y = FC_Config.RC.thrMid8;
        }

        lookupThrottleRC[i] = 10 * FC_Config.RC.thrMid8 + tmp * ( 100 - FC_Config.RC.thrExpo8 + (int32_t)FC_Config.RC.thrExpo8 * (tmp * tmp) / (y * y) ) / 10; // [0;1000]
        lookupThrottleRC[i] = FC_Config.MISC.minthrottle + (int32_t)(MAXTHROTTLE - FC_Config.MISC.minthrottle) * lookupThrottleRC[i] / 1000;  // [0;1000] -> [FC_Config.MISC.minthrottle;MAXTHROTTLE]
    }
}

/*******************************************************************************
* Function Name: FlightController_LoadConfiguration
********************************************************************************
* Summary:
*  Loads configuration and calibration data from EEPROM. If integrity check fails
*  for at least one config or the revision number does not match - reload all
*  configuration sets with the default values.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_LoadConfiguration(void)
{
    uint8_t currentSet;

    /* Read Config Info */
    EEPROM_ReadConfigInfo();

    /* Check if the correct config set was read */
    if (ConfigInfo.activeSet > FC_CONFIG_MAX_SET_NUMBER - 1)
    {
        ConfigInfo.activeSet = FC_CONFIG_DEFAULT_SET;
        /* Update ConfigInfo in the EEPROM */
        EEPROM_UpdateConfigInfo();
    }

    /* Check if the config revision match and integrity check pass */
    if ((ConfigInfo.revision != FC_CONFIG_REVISION) || (FC_VerifyConfigIntegrity() == false))
    {
        /* Revision mismatch means that we need to force update all config sets
         * in the EEPROM  with the default values */
        ConfigInfo.revision = FC_CONFIG_REVISION;
        EEPROM_UpdateConfigInfo();

        /* Read Data From EEPROM */
        FC_LoadConfigDefaults();

        for (currentSet = 0; currentSet < FC_CONFIG_MAX_SET_NUMBER; currentSet++)
        {
            /* Write Params and Blink the LED to Indicate Update */
            EEPROM_WriteParams(currentSet, 1);
        }
    }
    else
    {
        /* Load Configuration set selected in ConfigInfo */
        EEPROM_ReadParams(ConfigInfo.activeSet);
    }

    /* Fill In RC LUTs */
    FC_CalculateRCLookupVectors();

#if GPS_INSTALLED
    /* Set GPS PIDs */
    GPS_SetPIDs();    /* at this time we don't have info about GPS init done */
#endif /* GPS_INSTALLED */

    /* Load Calibration Data */
    if (EEPROM_ReadCalibrationData() != true)
    {
        /* Checksum mismatch was detected - initilaize the structure with the default values */
        FC_LoadCalibrationDefaults();

        /* Write default values to EEPROM */
        EEPROM_WriteCalibrationData();
    }
}
